/*
 *
 * PID.pde implementa un control PID (Proporcional Integral Derivativo)
 * mediante un chip PIC 18F2550/18F4550 y las librerias y el bootloader de pinguino proyect (www.pinguino.cc).
 * Asume que existe un servomotor de modelismo para el comando del actuador y un sensor de estado que genera
 * pulsos con una frecuencia proporcional al estado del sistema. Cada cierto lapso de tiempo se toma muestras
 * del estado relativo del sistema calulando la cantidad de pulsos en el periodo de tiempo
 *
 * Se cuenta con cinco entradas de control, una entrada set que activa el control y establece el setpoint al
 * estado actual. Una entrada resume que activa el control y utiliza el ultimo setpoint.
 * Dos entradas de modificacion up y down que aumentan y dusminuyen el setpoint en UP_RATIO y DOWN_RATIO
 * respectivamente.
 * Una entrada de stop que detiene el control moviendo el actuador a la posicion de reposo.
 * Un nivel alto en la entrada set cuando el control ya se encuentra activo lo desactiva
 *
 * @author Pato Silva
 * @date 29 de mayo de 2012
 * @version 0.3.0
 *
 */



/*
 * DEFINICION DE CONSTANTES
 */

/* Version (tomada del SVN... cuando me acuerdo) - valor de variable r20 */
#define FIRMWARE_VERSION 20

/* Pin de la señal de control del servo*/
#define ACTUATOR_PIN 11

/* Pin de entrada analogica desde el sensor de posicion del actuador - los pines del 13 al 17 pueden leer datos analogicos */
#define ACTUATOR_STATE_PIN 13

/* Pin de entrada analogica desde el sensor de estado actual, donde se leen los pulsos desde el sensor de velocidad */
#define ACTUAL_STATE_PIN 14

/* Cantidad de bits en la eeprom asignadas a almacenar datos de configuracion */
#define EEPROM_CONFIGURATION_BYTES 10

/* Tamaño del mapa en la eeprom que relaciona la posicion del actuador con el estado actual */
#define ACTUATOR_MAP_BYTES 160

/* Cantidad de bits en la eeprom asignadas a almacenar la configuracion del proceso pid */
#define PID_CONFIGURATION_BYTES 8

/* Pin de seteo, cunado es HIGH es porque se presiono el boton de seteo, se establece el setpoint y se activa el control */
#define SET_PIN 2

/* Pin de resume, indica que se debe activar el control*/
#define RESUME_PIN 3

/* Pin de incremento, incrementa setpoint en UP_RATIO porciento */
#define UP_PIN 4

/* Pin de decremento, decrementa setpoint en DOWN_RATIO, Pin 5 es PGM, no se puede utilizar con LVP */
#define DOWN_PIN 6

/* Pin de stop, detiene el control */
#define STOP_PIN 7

/* Proporcion en el que se aumenta el setpoint por cada vez que se activa la entrada UP */
#define UP_RATIO 1.03

/* Proporcion en el que se disminuye el setpoint por cada vez que se activa la entrada DOWN */
#define DOWN_RATIO 1.03

/* Pin indicador, marca que el control esta activo */
#define CONTROL_ACTIVE_LED_PIN 10

/* Lapso de tiempo (en milisegundos) en el que se toman muestras de estado, un valor bajo es mas rapido, un valor alto es mas preciso */
#define SPEED_MEASUREMENT_TIME 100

/* Tiempo de espera despues de enviar un dato por le USB */
#define USB_DELAY_AFTER_SEND 5


/*
 * DEFINICION DE HEADERS DE FUNCIONES
 */

/* Activa o desactiva el control pid segun el valor de la variable pasada por parametro */
void setControlActive(boolean is_active);

/* Mueve el actuador si la posicion ha cambiado */
void moveActuator();

/* Ejecuta un comando pasado por la conexión USB */
void commandExcecute(u8 *cmd);

/* Algoritmo del lazo pid */
int PIDControl(int setpoint, int actual_state);

/* Lee y busca en la eeprom la posicion en que debe estar el actuador para posicionarse en la posicion actual */
int readActuatorMap(int act_pos);

/* Envia a la salida de usb el dato pasado por parametro, si el parametro timestamp es true se agrega una marca de tiempo al final */
void logger(uchar *text, int length, boolean timestamp);

/* Envia al puerto USB el estado de la eeprom */
void dumpEeprom();

/* Envia un entero por la salida USB, que por ahora es una limitacion de la libreria de pinguino */
void USB_sendint(int value);

/* Envia un float por la salida USB */
void USB_sendfloat(float value);

/*Ejecuta un comando de seteo de datos */
void excecuteSet(u8 *cmd);

/*Ejecuta un comando de obtencion de datos */
void excecuteGet(u8 *cmd);


/*
 * DEFINICION DE VARIABLES
 */

/* Contador de pulsos del medidor de estado actual, cada SPEED_MEASURAMENT_TIME segundos se vuelve a cero*/
u16 pulse_counter = 0;

/* rw - 1- Un valor mas alto a esto en el pin ACTUAL_STATE_PIN (10 bits) se considera nivel alto */
u16 high_pulse_value = 500;

/* rw - 2- El valor mas alto detectado en el pin ACTUAL_STATE_PIN (10 bits) */
u16 highest_pulse_value = 0;

/* rw - 3- Un valor mas bajo detectado en el pin ACTUAL_STATE_PIN (10 bits) */
u16 lowest_pulse_value = 1024;

/* Valor actual del pulso leido en ACTUAL_STATE_PIN (10 bits) */
u16 current_pulse_value = 0;

/* Momento (retornado por millis()) en el que se tomo la ultima muestra de estado*/
u32 last_measurement = 0;

/* rw - 4- estado actual del sistema, segun el medidor de pulsos */
u16 actual_state = 0;

/* rw - 5- Posicion actual del servo actuador, en grados. Cuando el control esta inactivo actuator_position = 0, el maximo es ACTUATOR_MAP_BYTES grados */
u8 actuator_position = 0;

/* Posicion anterior del servo de control de acelerador, para detectar si la posicion ha cambiado */
u8 previous_actuator_position = 0;

/* rw - 6- setpoint seteada */
u16 setpoint = 0;

/* rw - 7- Indica si el circuito ya esta configurado para usarse, no se iniciara un proceso de control de no ser asi */
boolean is_configured = false;

/* rw - 8- Indica si el control activo en un momento dado */
boolean is_active = false;

/* Indica si el control estaba activo antes, el uso de dos banderas y 4 estados es para detectar si cambió el valor de la variable is_active,
 * parece una complicacion pero facilita las cosas ya que activar o desactivar el control (mover el servo, inicializar, prender leds) se hace
 * de forma aislada, un proceso (boton, error, entrada usb) lo activa y luego el bucle hace efectiva la activacion. Creeme que asi es mas facil */
boolean was_active = false;

/* Mantiene el estado del contador de pulsos para incrementarlo, cuando ACTUAL_STATE_PIN tiene un nivel alto y is_counter_pin_high es false
 * se le suma uno a pulse_counter y se pasa is_counter_pin_high a true, cuando ACTUAL_STATE_PIN es bajo hace is_counter_pin_high = false de
 * nuevo e incrementa pulse_counter de nuevo, suma uno por cada cambio de estado */
boolean is_counter_pin_high = false;

/* Indica si se debe ignorar un valor alto en el boton set/stop porque la pulsacion ya fue procesada */
boolean set_pressed = false;

/* Indica si se debe ignorar un valor alto en el boton resume porque la pulsacion ya fue procesada */
boolean resume_pressed = false;

/* Indica si se debe ignorar un valor alto en el boton up porque la pulsacion ya fue procesada */
boolean up_pressed = false;

/* Indica si se debe ignorar un valor alto en el boton down porque la pulsacion ya fue procesada */
boolean down_pressed = false;

/* rw - 9- El valor de actual_state (valor de pulse_counter SPEED_MEASUREMENT_TIME tiempo) se multiplica por este coheficiente, es para ajustar el
 * lazo PID y que se comporte de la misma manera aún cuando el sensor de estado actual emite mas o menos pulsos para un mismo estado */
float speed_adjustment_coefficient = 1;

/* Momento (retornado por millis()) del ultimo ingreso al control PID*/
unsigned long last_pid_execution = 0;

/* r - 10- Error anterior */
float last_error = 0;

/* r - 11- Valor de la proporcional del proceso PID */
float proporcional = 0;

/* r - 12- Valor de la integrative del proceso PID */
float integrative = 0;

/* r - 13- Valor de la derivative del proceso PID */
float derivative = 0;

/* rw - 14- tiempo entre un periodo de control y otro, para los lazos PID que controlan sistema lentos el valor de 1 segundo es generalmente adecuado. */
u16 Dt = 1000;

/* rw - 15- Valor Kp (proporcional) del control pid*/
float Kp = 0.1;

/* rw - 16- Valor Ki (integrative) del control pid*/
float Ki = 0.01;

/* rw - 17- Valor Kd (derivative) del control pid*/
float Kd = 0.005;

/* rw - 18- Debug activado o no, envia a la salida informacion de estado*/
boolean DEBUGGING = false;





/* Funcion de inicializacion */
void setup(){
  logger("Iniciando...\n", 13, true);

  logger("Chequeando configuracion ", 25, true);
  /* Chequea que el circuito ya este configurado, las primeras EEPROM_CONFIGURATION_BYTES posiciones de la eeprom indican si cierto parametro esta
   * configurado o no.
   * Si esta configurado las posiciones del EEPROM_CONFIGURATION_BYTES-1 a (EEPROM_CONFIGURATION_BYTES+ACTUATOR_MAP_BYTES-1) contiene la correspondencia
   * entre la posicion del servo (en grados) y el estado leido en ACTUATOR_STATE_PIN */
  if(EEPROM.read8(0) == 170){  // 10101010 ;)
    is_configured = true;
    logger("[  OK  ]\n", 9, false);
  }else{
    logger("[ FAIL ]\n", 9, false);
  }


  /* Los valores de Kp, Ki, Kd y Dt, si se configuraron (eeprom pos 1 = 170),
   * estan 10 posiciones a partir de la posicon de ACTUATOR_MAP_BYTES+EEPROM_CONFIGURATION_BYTES
   * son las posicones +10 y +11 para Kp, +12 y +13 para Ki, +14 y +15  para Kd y +16 y +17 para Dt */
  logger("Chequeando configuracion del algoritmo P.I.D. ", 46, true);
  if(EEPROM.read8(1) == 170){  // 10101010 ;)
    Kp = (EEPROM.read16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES)/1000);
    Ki = (EEPROM.read16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES + 2)/1000);
    Kd = (EEPROM.read16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES + 4)/1000);
    Dt = EEPROM.read16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES + 6);
    logger("[  OK  ]\n", 9, false);
  }else{
    logger("[ FAIL ]\n", 9, false);
  }


  /* Chequea que el coheficiente de velocidad este configurado*/
  logger("Chequeando configuracion del otros parametros ", 46, true);
  if(EEPROM.read8(2) == 170){  // 10101010 ;)
    speed_adjustment_coefficient = EEPROM.read16(EEPROM_CONFIGURATION_BYTES + ACTUATOR_MAP_BYTES + PID_CONFIGURATION_BYTES)/1000;
    high_pulse_value = EEPROM.read8(EEPROM_CONFIGURATION_BYTES + ACTUATOR_MAP_BYTES + PID_CONFIGURATION_BYTES + 1);
    logger("[  OK  ]\n", 9, false);
  }else{
    logger("[ FAIL ]\n", 9, false);
  }


  /* Asigna el pin al servo*/
  servo.attach(ACTUATOR_PIN);
  servo.write(ACTUATOR_PIN, actuator_position);


  /* Setea los pin de control a INPUT y el pin del led activo a OUTPUT */
  pinMode(SET_PIN, INPUT);
  pinMode(RESUME_PIN, INPUT);
  pinMode(UP_PIN, INPUT);
  pinMode(DOWN_PIN, INPUT);
  pinMode(STOP_PIN, INPUT);
  pinMode(CONTROL_ACTIVE_LED_PIN, OUTPUT);

  /* Inicializo el tiempo de ultima lectura al millis() actual */
  last_measurement = millis();

  /* Inicializo el tiempo de ultimo control pid al millis() actual */
  last_pid_execution = millis();
}



/* Funcion de repeticion */
void loop(){
  /* Cada cierto tiempo el valor retornado por millis() se reinicia y vuelve a cero, si se llegara a eso se resetea el valor de ultima medicion */
  if(millis() < last_measurement || millis() < last_pid_execution){
    last_measurement = millis();
    last_pid_execution = millis();
    pulse_counter = 0;
  }

  /* Calculo de velodicad actual, si corresponde */
  if(millis()-last_measurement >= SPEED_MEASUREMENT_TIME){
    actual_state = ((pulse_counter*SPEED_MEASUREMENT_TIME)/(millis()-last_measurement))*speed_adjustment_coefficient;
    pulse_counter = 0;
    last_measurement = millis();
  }

  /* actualizo el contador de pulsos */
  current_pulse_value = analogRead(ACTUAL_STATE_PIN);
  if(current_pulse_value >= high_pulse_value){
    if(!is_counter_pin_high){
     pulse_counter++;
     is_counter_pin_high = true;
    }
  }else{
   if(is_counter_pin_high){
     pulse_counter++;
     is_counter_pin_high = false;
    }
  }

  /* Actualizo highest y lowest*/
  if(current_pulse_value > highest_pulse_value){
    highest_pulse_value = current_pulse_value;
  }
  if(current_pulse_value < lowest_pulse_value){
    lowest_pulse_value = current_pulse_value;
  }

  /* si se presiona el boton set/stop */
  if(digitalRead(SET_PIN) == HIGH){
    if(!set_pressed && is_active){                        // Esta activo, se debe desactivar
      is_active = false;
      set_pressed = true;
    }else{
      if(is_configured && !set_pressed && actual_state > 0){  // activa solo si setpoint > 0 y no hay que ignorar el boton
        setpoint = actual_state;
        is_active = true;
        set_pressed = true;
      }
    }
  }else{
    set_pressed = false;
  }


  /* si se presiona el boton resume */
  if(digitalRead(RESUME_PIN) == HIGH){
    if(!resume_pressed && !is_active && setpoint > 0){  // activa solo si setpoint > 0 y no hay que ignorar el boton
      is_active = true;
      resume_pressed = true;
    }
  }else{
    resume_pressed = false;
  }

  /* si se presiona el boton up */
  if(digitalRead(UP_PIN) == HIGH){
    if(!up_pressed && is_active ){  // aumenta setpoint solo si no hay que ignorar el boton
      setpoint *= UP_RATIO;
      up_pressed = true;
    }
  }else{
    up_pressed = false;
  }

  /* si se presiona el boton down */
  if(digitalRead(DOWN_PIN) == HIGH){
    if(!down_pressed && is_active){  // disminuye setpoint solo si no hay que ignorar el boton
      setpoint /= DOWN_RATIO;
      down_pressed = true;
    }
  }else{
    down_pressed = false;
  }


  /* Seal de stop, detiene el control */
  if(digitalRead(STOP_PIN) == HIGH){      // Esta activo, se debe desactivar
    is_active = false;
  }


  /* Hace efectiva la activacion o desactivacón del lazo de control segun el valor de la variable is_active*/
  setControlActive(is_active);

  /* Mueve el servo, lo hace solo si cambió el valor de actuator_position */
  moveActuator();


  /*
   * Hace el ajuste del control PID (proporcional, integrativo y derivativo)
   */
  if(is_active && millis()-last_pid_execution > Dt){
    actuator_position = PIDControl(setpoint, actual_state);
  }

  /*
   * Ejecuta un comando pasado por la conexión USB
   */
  if(USB.available()){
    u8 cmd[5];
    u8 cmd_count = 0;
    u8 the_data;
    while(USB.available()){
        the_data = USB.read();
        if(cmd_count <= 5){
            cmd[cmd_count] = the_data;
        }
        cmd_count++;
    }
    if(cmd_count == 5){
        commandExcecute(cmd);
    }else{
        USB.send("nak", 3);
        USB_flush();
    }
  }
}


/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */


/* Activa o desactiva el control de crucero modificando las variables necesarias */
  void setControlActive(boolean state){
   if(was_active != state) {        // Cambió el estado?
     if(state){
         actuator_position = readActuatorMap(analogRead(ACTUAL_STATE_PIN));
         servo.write(ACTUATOR_PIN, actuator_position);                            // Mueve el servo a la posicion donde se encuentra el acelerador en ese momento segun la info guardada en la eeprom
         digitalWrite(CONTROL_ACTIVE_LED_PIN, HIGH);
         integrative = 0;
         last_error = 0;
     }else{               // Esta activo, se debe desactivar
        actuator_position = 0;
        servo.write(ACTUATOR_PIN, actuator_position);
        digitalWrite(CONTROL_ACTIVE_LED_PIN, LOW);
        integrative = 0;
        last_error = 0;
     }
     was_active = state;
   }
}



/* Mueve el servo a la posicion indicada si la misma ha cambiado */
void moveActuator(){
  if(previous_actuator_position != actuator_position){
    servo.write(ACTUATOR_PIN, actuator_position);
    previous_actuator_position = actuator_position;
  }
}



 /* Actua segun los comandos que se reciban, todo comando cuenta de 5 bytes, empieza con una cabecera y termna con la misma cabecera pero los bytes negados
  * Si el primer byte es un 0xAA (10101010) es un seteo, el segundo byte dice que variable setear y los siguientes 2 dan el valor
  * Por ejemplo "0xAA 0x02 0x00 0x6E 0x55" establece la variable 2 en 110, el 0x55 es !0xAA
  * mientras que "0xAA 0x14 0x4F 0xA1 0x55" establece la posicion 79 (4F) de la variable 20 (14Hex = 20Dec) en 161 (A1)
  *
  * Si el primer byte es un 0xF0 (11110000) es una lectura, el segundo byte dice que variable reportar, el tercero la posicion (si la
  * variable fuera un arreglo o la eeprom)
  * Si no se tiene esa estructura el comando no se ejecuta, se responde con null 0x00 y se limpia el buffer durante USB_DELAY_AFTER_SEND milisengundos
  */
  void commandExcecute(u8 *cmd){
      if(cmd[0] == 0xAA && cmd[4] == 0x55){
        excecuteSet(cmd);
      }else{
        if(cmd[0] == 0xF0 && cmd[4] == 0x0F){
            excecuteGet(cmd);
        }else{
            USB.send("nak", 3);
        }
      }
  }

    /*
     * Ejecuta un comando de seteo
     */
    void excecuteSet(u8 *cmd){
        switch(cmd[1]){
          case 1:{
            high_pulse_value = (cmd[2] << 8) + cmd[3];
            break;
          }
          case 2:{
            highest_pulse_value = (cmd[2] << 8) + cmd[3];
            break;
          }
          case 3:{
            lowest_pulse_value = (cmd[2] << 8) + cmd[3];
            break;
          }
          case 4:{
            actual_state = (cmd[2] << 8) + cmd[3];
            break;
          }
          case 5:{
            actuator_position = (cmd[2] << 8) + cmd[3];
            break;
          }
          case 6:{
            setpoint = (cmd[2] << 8) + cmd[3];
            break;
          }
          case 7:{
            is_configured = (cmd[2] == 1);
            break;
          }
          case 8:{
            is_active = (cmd[2] == 1);
            break;
          }
          case 9:{
            speed_adjustment_coefficient = ((cmd[2] << 8) + cmd[3])/1000;
            break;
          }
          case 14:{
            EEPROM.write16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES + 6, ((cmd[2] << 8) + cmd[3]));
            Dt = EEPROM.read16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES + 6);
            break;
          }
          case 15:{
            EEPROM.write16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES, ((cmd[2] << 8) + cmd[3]));
            Kp= EEPROM.read16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES);
            break;
          }
          case 16:{
            EEPROM.write16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES + 2, ((cmd[2] << 8) + cmd[3]));
            Ki= EEPROM.read16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES + 2);
            break;
          }
          case 17:{
            EEPROM.write16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES + 4, ((cmd[2] << 8) + cmd[3]));
            Kd= EEPROM.read16(ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES + 4);
            break;
          }
          case 18:{
            DEBUGGING = (cmd[2] == 1);
            break;
          }
          case 30:{
            EEPROM.write(cmd[2], cmd[3]);
            break;
          }
          default:{
            break;
          }
        }
    }

    /*
     * Ejecuta un comando de obtencion de datos
     */
    void excecuteGet(u8 *cmd){
         switch(cmd[1]){
          case 1:{
            USB_sendint(high_pulse_value);
            break;
          }
          case 2:{
            USB_sendint(highest_pulse_value);
            break;
          }
          case 3:{
            USB_sendint(lowest_pulse_value);
            break;
          }
          case 4:{
            USB_sendint(actual_state);
            break;
          }
          case 5:{
            USB.send(actuator_position, 1);
            break;
          }
          case 6:{
            USB_sendint(setpoint);
            break;
          }
          case 7:{
            USB.send(is_configured, 1);
            break;
          }
          case 8:{
            USB.send(is_active, 1);
            break;
          }
          case 9:{
            USB_sendfloat(speed_adjustment_coefficient);
            break;
          }
          case 10:{
            USB_sendfloat(last_error);
            break;
          }
          case 11:{
            USB_sendfloat(proporcional);
            break;
          }
          case 12:{
            USB_sendfloat(integrative);
            break;
          }
          case 13:{
            USB_sendfloat(derivative);
            break;
          }
          case 14:{
            USB_sendint(Dt);
            break;
          }
          case 15:{
            USB_sendfloat(Kp);
            break;
          }
          case 16:{
            USB_sendfloat(Ki);
            break;
          }
          case 17:{
            USB_sendfloat(Kd);
            break;
          }
          case 18:{
            USB.send(DEBUGGING, 1);
            break;
          }
          case 20:{
            USB_sendint(FIRMWARE_VERSION);
            break;
          }
          case 30:{
            USB.send(EEPROM.read8(cmd[2]), 1);
            break;
          }
          default:{
            break;
          }
        }
    }



  /* Envia a la salida de usb el dato pasado por parametro, si el parametro timestamp es true se agrega una marca de tiempo al final */
  void logger(uchar *text, int length, boolean timestamp){
    if(DEBUGGING){
        USB.send(text, length);
        delay(USB_DELAY_AFTER_SEND);
     }
  }



/*
 * Ajuste mediante el metodo PID
 * Set point es el estado que se desea alcanzar, actual point es el estado actual
 *
 * Los valores de ajuste por defecto son Kp = 0.1, Kd = 0.01 y Ki = 0.005, despues veremos como lo mejoramos
 *
 * La proporcional dice que servo = set_point. Simplemente supone qe para un servo a 100 el estado es 100
 * Para que esto aproxime medianamente bien la variable de ajuste de velocidad debe estar configurada
 * Se puede pensar en mejorar esto tomando tres puntos reales conocidos e interpolarlo por lagrange.
 */
int PIDControl(int set_point, int actual_point){
  float error;
  int output;

  /* Error de velocidad actual*/
  error = set_point - actual_point;

  /* Calculo del proporcional */
  proporcional = set_point + (Kp*error);

  /* En caso de que el error sea aun muy pequeño no se aplica integrative, si integrative no está en el rango [-60;60] se
   * reduce a esos limites (windup)
   */
    if(abs(error) > 0.1){
        integrative = Ki*(integrative + error);
        if(integrative > 60){
          integrative = 60;
        }else{
          if(integrative < -60){
            integrative = -60;
          }
        }
    }

  // Calculo del derivativo
  derivative = Kd*(error - last_error);

  // Calculo el nuevo valor del servo
  output = proporcional + integrative + derivative;

  // almaceno error para los proximos cambios
  last_error = error;

 return output;
}




/*
 * Funcion readActuatorMap retorna la posicion en grados en la que se debe ubicar el actuador para que la posicion corresponde con el valor del estado
 * actual del actuador leido por el pin ACTUATOR_STATE_PIN.
 * Se la utiliza al activar el control, resulta un tanto ineficiente ya que se debe leer de la EEPROM y el valor debe ser buscado mediante busqueda binaria.
 * Parametro act_pos es el valor leido en el puerto ACTUATOR_STATE_PIN, como es un valor de 10 bits y la eeprom solo almacena los 8 mas significativos
 * Retorna el angulo que debe tomar el actuador para estar en esa posicion
 * Comienza en la posicion 10 ya que las primeras 10 se utilizan de banderas de configuración
 */
int readActuatorMap(int act_pos){
  u8 mapped_pos = (u8)(act_pos/4); // mapea el valor de 10 bits a uno de 8 bits, para este caso solo hace act_pos/4
  u8 arriba = ACTUATOR_MAP_BYTES + EEPROM_CONFIGURATION_BYTES - 1;     // El primer datos de correspondencia se encuentra en la posicion EEPROM_CONFIGURATION_BYTES
  u8 abajo = EEPROM_CONFIGURATION_BYTES;
  u8 centro;
  u8 valor;
  while (abajo <= arriba){
    centro = (arriba + abajo)/2;
    valor = EEPROM.read8(centro);
    if (valor == mapped_pos)
      return centro;
    else
      if (mapped_pos < valor)
        arriba = centro-1;
      else
        abajo = centro+1;
    }
  return centro;  // Si no existe el valor exacto retorno el mas cercano
 }



/* Envia al puerto USB el estado de la eeprom como un entero de 2 bytes, los bytes mas significativos son el indice en
 * la eeprom y los menos significativos el valor leído
 */
void dumpEeprom(){
  u16 i;
    for (i=0;i<255;i++){
        USB_sendint(i << 8 + EEPROM.read(i));
        delay(USB_DELAY_AFTER_SEND);
    }
}



/* Envia un entero por la salida USB, que por ahora es una limitacion de la libreria de pinguino */
void USB_sendint(int value){
    int val[1];
    val[0] = value;
    USB.send((uchar *)val, 2);
}



/* Creeria que envia un float por la salida USB, ni la menor idea si funciona */
void USB_sendfloat(float value){
    float val[1];
    val[0] = value;
    USB.send((uchar *)val, 4);
}

/* Vacia el buffer USB */
void USB_flush(){
  delay(USB_DELAY_AFTER_SEND);
  while(USB.available() > 0){
     USB.read();
  }
  delay(USB_DELAY_AFTER_SEND);
}
